#include "esp_timer.h"
#include "math_common.h"

const static _iq24 _1_e_3              = _IQ(1e-3);
const static _iq24 _zero_3             = _IQ(0.3);

const static _iq24 meanFilterArray[10] = {_IQ(1.F / 1.F), _IQ(1.F / 2.F), _IQ(1.F / 3.F), _IQ(1.F / 4.F),
                                          _IQ(1.F / 5.F), _IQ(1.F / 6.F), _IQ(1.F / 7.F), _IQ(1.F / 8.F),
                                          _IQ(1.F / 9.F), _IQ(1.F / 10.F)};

typedef struct
{
    int    winSize;
    int    index;
    int    isFull;
    _iq24  sum;
    _iq24 *data;
} MeanFilter;

typedef struct
{
    _iq24 prevData;
    _iq24 alpha;
    // uint64_t prevTs;
} LowPassFilter;

typedef struct
{
    MeanFilter    mean;
    LowPassFilter lowPass;
} FilterObj;


static _iq24 IRAM_ATTR FILTER_slideMean(FilterAlgorithm *alg, _iq24 data)
{
    FilterObj *priv = NULL;

    if (!alg)
    {
        return data;
    }

    priv           = alg->priv;
    priv->mean.sum -= priv->mean.data[priv->mean.index];
    priv->mean.sum += data;
    /*记录数据*/
    priv->mean.data[priv->mean.index++] = data;
    if (priv->mean.index == priv->mean.winSize)
    {
        priv->mean.isFull = 1;
        priv->mean.index  = 0;
    }

    if (!priv->mean.isFull)
    {
        return _IQmpy(priv->mean.sum, meanFilterArray[priv->mean.index - 1]);
    }

    return _IQmpy(priv->mean.sum, meanFilterArray[priv->mean.winSize - 1]);
}

static _iq24 IRAM_ATTR FILTER_lowPass(FilterAlgorithm *alg, _iq24 data)
{
    FilterObj *priv  = NULL;
    _iq24      out   = data;

    /*获取时间*/
    // ts = esp_timer_get_time();
    if (!alg)
    {
        return out;
    }

    priv = alg->priv;
    out  = _IQmpy(priv->lowPass.alpha, priv->lowPass.prevData) + _IQmpy(_one - priv->lowPass.alpha, data);
    // printf("prev = %f,data=%f,alpha=%f,ts=%f\n",priv->lowPass.prevData,data,alpha,td);
    /*记录当前滤波数据*/
    // priv->lowPass.prevTs   = ts;
    priv->lowPass.prevData = out;
    return out;
}

void FILTER_delete(FilterAlgorithm *alg)
{
    FilterObj *priv = NULL;

    if (!alg)
    {
        return;
    }

    priv = alg->priv;
    if (priv)
    {
        if (priv->mean.data)
        {
            free(priv->mean.data);
        }
        free(priv);
    }

    free(alg);
}

esp_err_t FILTER_creat(FilterAlgorithm **alg, FilterCfg cfg)
{
    FilterAlgorithm *filter = NULL;
    FilterObj       *priv   = NULL;

    /*申请内存*/
    filter = calloc(1, sizeof(FilterAlgorithm));
    if (!filter)
    {
        return ESP_FAIL;
    }
    priv = calloc(1, sizeof(FilterObj));
    if (!priv)
    {
        goto free;
    }
    filter->priv        = priv;
    priv->mean.winSize  = cfg.meanWinSize;
    priv->lowPass.alpha = _IQdiv(cfg.lowPassAlpha, cfg.lowPassAlpha + cfg.ts);
    /*申请窗口内存*/
    priv->mean.data = calloc(priv->mean.winSize, sizeof(double));
    if (!priv->mean.data)
    {
        goto free;
    }

    filter->slideMeanFilter = FILTER_slideMean;
    filter->lowPassFilter   = FILTER_lowPass;

    /*输出滤波对象*/
    *alg = filter;
    return ESP_OK;

free:
    FILTER_delete(filter);
    return ESP_FAIL;
}